#include <math.h>
#include "distances.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
//#include "std_msgs/Header.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Float64MultiArray.h"
#include "pi_tracker/Skeleton.h"
#include "distances/DistancesMatrix.h"
#include <vector>
#include <iostream>


int amtime;
ros::NodeHandle *pn;
ros::Publisher *pp;

int interval;
int isec;
int insec;
int secs2;
int secs1;
int nsecs2;
int nsecs1;
int imp;
int ut;
//int count=0;
class itemClass
{
public:
  float matrix[15][15];
  int sec;
  int nsec;
  
};



itemClass item;
std::vector<itemClass> meanvector;
float mean[15][15];



float distance(float *a, float *b){
  
  return
  sqrt((a[0]-b[0])*(a[0]-b[0])+(a[1]-b[1])*(a[1]-b[1])+(a[2]-b[2])*(a[2]-b[2]));
}

float skeletonDistance(pi_tracker::Skeleton::ConstPtr msg, int a, int b){
  
  float u[3];
  float v[3];
  
  u[0]=(float) msg->position[a].x;
  u[1]=(float) msg->position[a].y;
  u[2]=(float) msg->position[a].z;
  
  v[0]=(float) msg->position[b].x;
  v[1]=(float) msg->position[b].y;
  v[2]=(float) msg->position[b].z;
  
  return distance(u,v);
  
}

void Callback(const pi_tracker::Skeleton::ConstPtr &msg)
{ static int count=0;
//passa o valor do parametro amtime pra variável int amtime
ros::param::param("amtime", amtime, 1000);
std::cout << amtime << std::endl;

float distances[15][15];

using namespace nsJoints;

distances::DistancesMatrix matrix;
//distances::DistancesMatrix meanmatrix;
distances::Vector vector;
matrix.dM.clear();

for(int i=0; i<15;i++){
  vector.v.clear();
  for(int j=0; j<15;j++){
    distances[i][j]=skeletonDistance(msg,i,j);
    item.matrix[i][j]=distances[i][j];
    //std::cout<<item.matrix[i][j]<<" ";
    vector.v.push_back(distances[i][j]);
  }
  //std::cout<<std::endl;
  matrix.dM.push_back(vector);
  
}
secs2=(int)msg->header.stamp.sec;
nsecs2=(int)msg->header.stamp.nsec;
item.sec=secs2;
item.nsec=nsecs2;
meanvector.insert(meanvector.end(),item);

matrix.quant=0;
pp->publish(matrix);
ROS_INFO("I published an instantaneous distance matrix!");

 if(ut==1){
   secs1=(int)msg->header.stamp.sec;
   nsecs1=(int)msg->header.stamp.nsec;
   ut=0;
 }


 isec=secs2-secs1;
 insec=nsecs2-nsecs1;
 
 interval=isec*1000+insec/1000000;
if(interval>amtime){
  ut=1;
for (int i; i<meanvector.size(); i++ )
{  
  for(int i=0;i<15;i++)
  {
    for(int j=0;j<15;j++)
    {
      mean[i][j]=meanvector.at(i).matrix[i][j]+mean[i][j];
    }
  }
}
matrix.dM.clear();
  for(int i=0;i<15;i++)
  {
    vector.v.clear();
    for(int j=0;j<15;j++)
    {
      mean[i][j]=mean[i][j]/meanvector.size();
      vector.v.push_back(mean[i][j]);
    }
     matrix.mM.push_back(vector);
  }

matrix.quant=meanvector.size();
pp->publish(matrix);
ROS_INFO("I published a mean distance matrix!");
}
  
/*
 i f*(ut==1){
   secs1=(int)msg->header.stamp.sec;
   nsecs1=(int)msg->header.stamp.nsec;
   ut=0;
 }
 
 secs2=(int)msg->header.stamp.sec;
 nsecs2=(int)msg->header.stamp.nsec;
 
 isec=secs2-secs1;
 insec=nsecs2-nsecs1;
 
 interval=isec*1000+insec/1000000;
 if(interval>amtime){
   imp=1;
 }else{
   imp=0;
 }
 
 if(imp==0)
 {
   
   for(int i=0;i<15;i++)
   {
     for(int j=0;j<15;j++)
     {
       mean[i][j]=distances[i][j]+mean[i][j];
     }
   }
   
   count++;
 } else
 {
   
   
   matrix.mM.clear();
   
   for(int i=0; i<15;i++){
     vector.v.clear();
     for(int j=0; j<15;j++){
       vector.v.push_back(mean[i][j]/count);
     }
     
     matrix.mM.push_back(vector);
     
   }
   
   matrix.quant=count;
   
   pp->publish(matrix);
   ROS_INFO("I published a mean distance matrix!");
   
   count=0;
   ut=1;
   for(int i=0;i<15;i++)
   {
     for(int j=0;j<15;j++)
     {
       mean[i][j]=0;
     }
   }
   
 }*/




 }
 
 int main(int argc, char **argv)
 {
   /**right
    * The ros::init() function needs to see argc and argv so that it can perform
    * any ROS arguments and name remapping that were provided at the command
    * line. For programmatic
    * remappings you can use a different version of init() which takes remappings
    * directly, but for most command-line programs, passing argc and argv is the
    * easiest
    * way to do it.  The third argument to init() is the name of the node.
    *
    * You must call one of the versions of ros::init() before using any other
    * part of the ROS system.
    */
   ros::init(argc, argv, "distances");
   
   /**
    * NodeHandle is the main access point to communications with the ROS system.
    * The first NodeHandle constructed will fully initialize this node, and the
    * last
    * NodeHandle destructed will close down the node.
    */
   ros::NodeHandle n;
   pn=&n;
   
   /**
    * The subscribe() call is how you tell ROS that you want to receive messages
    * on a given topic.  This invokes a call to the ROS
    * master node, which keeps a registry of who is publishing and who
    * is subscribing.  Messages are passed to a callback function, here
    * called chatterCallback.  subscribe() returns a Subscriber object that you
    * must hold on to until you want to unsubscribe.  When all copies of the
    * Subscriber
    * object go out of scope, this callback will automatically be unsubscribed
    * from
    * this topic.
    *
    * The second parameter to the subscribe() function is the size of the message
    * queue.  If messages are arriving faster than they are being processed, this
    * is the number of messages that will be buffered up before beginning to
    * throw
    * away the oldest ones.
    */
   
   for(int i=0;i<15;i++){
     for(int j=0;j<15;j++){
       item.matrix[i][j]=0;
       mean[i][j]=0;
       
     }
   }
   ut=1;
   imp=0;
   ros::Subscriber sub = n.subscribe("skeleton", 1000, Callback);
   //ros::Subscriber sub = n.subscribe("/skeleton", 1000, ftest);
   
   
   ros::Publisher pub = n.advertise<distances::DistancesMatrix>("distances", 1000);
   
   pp=&pub;
   
   ros::Rate loop_rate(10);
   
   
   
   
   
   /**
    * ros::spin() will enter a loop, pumping callbacks.  With this version, all
    * callbacks will be called from within this thread (the main one). 
    * ros::spin()
    * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
    */
   ros::spin();
   
   return 0;
 } 